#include "chassis_power_control.h"
#include "referee.h"
#include "arm_math.h"

#define POWER_LIMIT         95.0f  // 最大功率
#define WARNING_POWER       80.0f  // 警告功率

#define BUFFER_TOTAL_CURRENT_LIMIT      16000.0f
#define POWER_TOTAL_CURRENT_LIMIT       20000.0f


supcap_ctrl_t supcap_status;

/**
  * @brief          限制功率，主要限制电机电流
  * @param[in]      chassis_power_control: 底盘数据
  * @retval         none
  */
void chassis_power_control(chassis_move_t *chassis_power_control)
{
    fp32 chassis_power = supcap_status.chassis_pow;
    fp32 total_current_limit = 0.0f;
    fp32 total_current = 0.0f;

    // 根据当前功率和最大功率限制电流-+9
    if (chassis_power > WARNING_POWER)
    {
        fp32 power_scale;

        // 功率小于最大功率
        if (chassis_power < POWER_LIMIT)
        {
            // 按比例缩小电流
            power_scale = (POWER_LIMIT - chassis_power) / (POWER_LIMIT - WARNING_POWER);
        }
        // 功率大于最大功率
        else
        {
            power_scale = 0.0f;
        }

        total_current_limit = BUFFER_TOTAL_CURRENT_LIMIT + POWER_TOTAL_CURRENT_LIMIT * power_scale;
    }
    // 功率小于警告功率
    else
    {
        total_current_limit = BUFFER_TOTAL_CURRENT_LIMIT + POWER_TOTAL_CURRENT_LIMIT;
    }

    // 计算原本电机电流设定
    for (uint8_t i = 0; i < 4; i++)
    {
        total_current += fabs(chassis_power_control->motor_speed_pid[i].out);
    }

    // 如果总电流超过限制，按比例缩小电机电流
    if (total_current > total_current_limit)
    {
        fp32 current_scale = total_current_limit / total_current;
        chassis_power_control->motor_speed_pid[0].out *= current_scale;
        chassis_power_control->motor_speed_pid[1].out *= current_scale;
        chassis_power_control->motor_speed_pid[2].out *= current_scale;
        chassis_power_control->motor_speed_pid[3].out *= current_scale;
    }
}